/*
 * aocs.c
 *
 *  Created on: 2025年6月17日
 *      Author: Administrator
 */


#include <stdio.h>
#include "control.h"
#include "st_dynamics_driver.h"
#include "dlx.h"

void aocs_main(void)
{
    //int_T T_Id=1;
    int i;
    
    /* ZHDZ receive buffer  */
    union Dyn_buffer  reBuffer;

    /* ZHDZ send buffer  */
    union Dyn_buffer  sdBuffer;

    sciSetBaudrate(sciREG4,115200);

    sdBuffer.cmd[0] = 0x28;
    sdBuffer.cmd[1] = 0x03;
    // for(i=2;i<75;i++)
    // {
    //     sdBuffer.cmd[i] = i;
    // }
    // ZHDZ send,  DLX receive
    sciSend(ST_DYN_SCI, 70, sdBuffer.cmd);

    /* aocs Control Algorithm */
    control();

    print("aocs system");


    // ZHDZ  receive,  DLX send
    print_mem(reBuffer.cmd, 50, 16);
    st_dyn_rx(reBuffer.cmd, 200);

    //bufferDlxSend = (Dyn_Send *) cmdR;

    //print("Spd: %.3f, %.3f, %.3f\n",  reBuffer.dynSendBuff.spd[0],  reBuffer.dynSendBuff.spd[1],  reBuffer.dynSendBuff.spd[2]);
    //print("Pos: %.3f, %.3f, %.3f\n",  reBuffer.dynSendBuff.pos[0],  reBuffer.dynSendBuff.pos[1],  reBuffer.dynSendBuff.pos[2]);
    //print("assA: %.3f, %.3f, %.3f\n", reBuffer.dynSendBuff.assA[0], reBuffer.dynSendBuff.assA[1], reBuffer.dynSendBuff.assA[2]);

    /*RCW*/
    for (i=0;i<16;i++)
    {
        sdBuffer.cmd[2+i] = reBuffer.cmd[188+i]; 
    }
    /*MT*/
    for (i=0;i<8;i++)
    {
        sdBuffer.cmd[18+i] = reBuffer.cmd[78+i];
    }
    for (i=0;i<4;i++)
    {
        sdBuffer.cmd[26+i] = reBuffer.cmd[130+i];
    }
    



}
